/*
 * Copyright (C) 2010 Johan Waldeback
 *  
 * This file is part of the OpenFMS project (http://www.openfms-project.org)
 * 
 * OpenFMS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * \class OpenFms::Fms7Aero
 * \brief Handles airplane models in the FMS version 7 format
 *
 * After loading a .par file. This class calculates how the model behaves in a given situation.
 * todo: longer description:
 *   forces are filled in given environment
 *   inertias can be retrived
 * @note
 * This class is not yet in use anywhere (but will be in the future).
 */

#include "Fms7Aero.h"
#include "OfmsMath.h"
using namespace OpenFms;

#include <stdint.h>
#include <cctype>
#include <cassert>
#include <iostream>
#include <cmath>

static const int CHANNEL_THRUST = 2;    /**< Selects which channel is used to control thrust */
static const int CHANNEL_ELEVATOR = 1;  /**< Selects which channel is used to control elevation */
static const int CHANNEL_ALERION = 0;   /**< Selects which channel is used to control alerion */
static const int CHANNEL_RUDDER = 3;    /**< Selects which channel is used to control rudder (yaw) */

using namespace std;
using namespace OpenFms;

/**
 * Converts a string read from a callback function to a float
 *
 * Note: either '.' or ',' can be used as a decimal point
 * Scientific notation (e.g. 1.23e-4) is allowed
 *
 * @param[out] result The resuling floatt of the parsing, only valid if function returns true
 * @param[in] read_fn The read callback function to read the string
 * @param[in] custom This value is assed on when calling the callback function
 * @return Returns true if a value was successfully parsed, false if not.
 */
bool Fms7Aero::readFloat( float &result, ModelSimulator_read_callback_t read_fn, uint64_t custom )
{
	const int buffer_len = 50;
	char buffer[buffer_len];
	char ch ;
	size_t index = 0;

	bool hasDigit = false;      // set to true when a digit has appeared
	bool eAllowed = true;       // an e or E is allowed
	bool signAllowed = true;    // a +/- sign is allowed?
	bool decimalAllowed = true; // a . allowed?
	bool foundEnd = false;

	bool neg = false;
	bool exp_neg = false;
	int numerator = 0;
	int denumerator = 0;
	int exponent = 0;
	
	for(;;)
	{
		if( read_fn(custom, reinterpret_cast<uint64_t>(&ch), 1) != 1 ) {
			foundEnd = true;
			break;
		}
		// on.fly translate: ',' -> '.'
		if( ch == ',' ) ch = '.'; 

		if( ch >= '0' && ch <= '9' ) {
			hasDigit = true;
			signAllowed = false;
			
			if( eAllowed ) { 
				numerator = numerator * 10 + static_cast<int>(ch-'0');
				denumerator *= 10;
			}
			else {
				exponent = exponent * 10 + static_cast<int>(ch-'0');
			}
		}
		else if( ch == '.' || ch ==',' ) {
			if( decimalAllowed == false ) return false;
			decimalAllowed=false;
			signAllowed = false;

			if( eAllowed ) denumerator = 1;
		}
		else if( ch =='e' || ch == 'E' ) {
			if( eAllowed == false ) return false;
			eAllowed = false;
			decimalAllowed = false;
			signAllowed = true;
		}
		else if( ch == '+' || ch == '-' ) {
			if( signAllowed == false ) return false;
			signAllowed=false;
			if( ch == '-' ) {
				if(eAllowed) neg = true;
				else exp_neg = true;
			}
		}
		else if( ch ==' ' || ch == '\t' || ch =='\r' || ch == '\n' || ch =='\v' || ch == '\f' ) {
			if( hasDigit == true ) break;
			continue; // no point in storing the leading whitespace in buffer
		}
		else {
			//if( hasDigit == true ) break;
			return false;
		}

		buffer[index++] = ch;

		// 3.123456789123456789.... (i.e. valid but too long), just break, and parse whats fits in the buffer. 
		if( index >= buffer_len-1 ) break;
	}

	// when here, it's time to parse.
	// however, if we havn't seen any digits it's an invalid.
	if( hasDigit == false ) return false;
	buffer[index] = 0; // add a terminating NULL

	// calculate fractional
	result =  static_cast<float>(numerator);
	if( denumerator >0 ) result /= denumerator;
	if( neg ) result = -result ;

	// calculate exponent
	if( exp_neg ) exponent = -exponent;
	for(; exponent > 0; --exponent ) { result *= 10; }
	for(; exponent < 0; ++exponent ) { result /= 10; }

	// flush stream until the end of line (or en of file)
	if( foundEnd == false ) {
		while ( ch != '\r' && ch != '\n' ) {
			if( read_fn(custom, reinterpret_cast<uint64_t>(&ch), 1) ==0 ) 
				break;
		}
	}

	return true;
}

/**
 * Fms7Aero constructor
 */
Fms7Aero::Fms7Aero () : m_stepSize(0.5f) {}

/**
 * Fms7Aero destructor
 */
Fms7Aero::~Fms7Aero() { this->destroy(); }

/**
 * Creates an instance of a model to simulate.
 *
 * @param readfn Callback function to use when reading data.
 * @param custom Custom value, is passsed on when calling the callback function.
 * @return Returns true on success, false on fail.
 */
bool Fms7Aero::create(ModelSimulator_read_callback_t readfn, uint64_t custom)
{
    float fType;

    // read first, should be "0" to comply
    if(!readFloat(fType, readfn, custom)) return false;
    if(fType!=0.f) return false;


    // read 26 lines, fill with zeros if not enough
    for(int i=0; i<26; i++) {
        if( readFloat(this->m_paramarray[i], readfn, custom) == false ) {

			// Must have found at least 25 datas
			if( i<25) return false;

            // fill up with zeros, and return success
            for(;i<26;++i) { this->m_paramarray[i]=0.f; }
			break;
        }
    }

    // if here, then there might be more than 26 lines,
	// but just ignore that - we've already got what we wanted
    return true;
}

/**
 * Frees all resources allocated by a previous call to \c create
 */
void Fms7Aero::destroy()
{
    // this class does not dynamically allocate any memory, so nothing to do here
}

/**
 * Get mass and inertia tensor
 *
 * Fills in a ModelSimulator_inertias_t structure with data. The size member of the 
 * structure must be set to a valid value.
 * @param inertias Structure to be filled in.
 * @return Returns true if successful, false if not.
 */
bool Fms7Aero::getInertias(ModelSimulator_inertias_t &inertias)
{
	// assert size memberis set to a valid value
	assert( inertias.size >= 20 );

	// fill in the values
	if( inertias.size >= 20 ) {
		inertias.mass = m_params.m;
		inertias.ixx  = m_params.ixx;
		inertias.iyy  = m_params.iyy;
		inertias.izz  = m_params.izz;
	}
	if( inertias.size >= 24 ) inertias.ixy  = 0.f;
	if( inertias.size >= 28 ) inertias.ixz  = 0.f;
	if( inertias.size >= 32 ) inertias.iyz  = 0.f;

	// retrun success
	return true;
}

/**
 * Set which stepsize to use in seconds
 *
 * @param stepSize to use
 * @return Returns true if successful, false if not.
 */
bool Fms7Aero::setStepSize(float stepSize)
{
    this->m_stepSize = stepSize;
    return true;
}

/**
 * Step the simulation one step
 *
 * Uses the given environment during the specified amount of time, and fills is the 
 * forces and torques which then acts on the model. The size member of the forces
 * structure must be set to a valid value.
 * 
 * @param[in] environment Constains the environment to use during the step.
 * @param[out] forces Placeholder for the result of the model simulation.
 * @return Retruns true if successful, false if not
 */
bool Fms7Aero::step(const ModelSimulator_environment_t &environment, ModelSimulator_forces_t &forces)
{
	assert( forces.size == 28 );
	//float alpha = -atan2( -environment.wind_z, -environment.wind_x ); // get apparent angle of attack (of airplane)
	//ModelSimulator_forces_t f;

	const btVector3 wind( environment.linearVelocityX, environment.linearVelocityY, environment.linearVelocityZ );

	//float thrust = m_params.thrust_max * toThrustScale( environment.channel[CHANNEL_THRUST] );
	float thrust = m_params.thrust_max * environment.channel[CHANNEL_THRUST];

	// todo: which one is she, and wich ne is sfe

	clearForces(forces);
	
	forces.size = 28;
	forces.force_x = thrust;
	forces.force_y = 0.f;
	forces.force_z = 0.f;//-5*thrust;
	forces.torque_x = -3.f* environment.angularVelocityX;
    forces.torque_y = -3.f* environment.angularVelocityY;
	forces.torque_z = -3.f* environment.angularVelocityZ;// + m_params.thrust_max * environment.channel[CHANNEL_TZ];


	/*const float rudderAngle = environment.channel[ CHANNEL_RUDDER ] * m_params.dr_max;
	btVector3 rudderNormal( sinf(rudderAngle), cosf(rudderAngle), 0.f );
	btVector3 rudderForce = forceOnSurface( rudderNormal, wind, m_params.she );
	addForce( forces, rudderForce, btVector3( -m_params.lt, 0, 0 ));*/

	/*const float elevatorAngle = environment.channel[ CHANNEL_ELEVATOR ] * m_params.de_max;
	btVector3 elevatorNormal( 0.f, cosf(elevatorAngle), -sinf(elevatorAngle) );
	btVector3 elevatorForce = forceOnSurface( elevatorNormal, wind, m_params.sfe );
	addForce( forces, elevatorForce, btVector3( -m_params.lt, 0, 0 ));*/

	return true;
}

/**
 * Calculates lift coefficien (C_L) for a specified angle of attack
 *
 * @param angleOfAttack Angle of attack (of airplane)
 * @return Resturns the list coefficient.
 */
float Fms7Aero::getLiftCoefficient( float angleOfAttack ){
	// todo: write this function
	// todo: shuld alpha0 be added in here ore outside?
	// TODO: how handle cl_max <= cl_min
	// todo: make better:
	// angle of attack -> [ -pi , pi ]
	angleOfAttack = atan2f( sinf(angleOfAttack), cosf(angleOfAttack));

	float cl = angleOfAttack * this->m_params.cl_a;

	// TODO: for now use infinite stall slope isf cl_a_stall
	if( cl > this->m_params.cl_max || cl < this->m_params.cl_min ) {
		cl = 0;
	}
	/*if( cl > this->m_params.cl_max ) {
		//float this->m_params.cl_a / this->m_params.cl_max
		// todo: if here, this->m_params.cl_a can't be zero ?
		float exess_angle = ( cl - this->m_params.cl_max ) / this->m_params.cl_a;
		//cl = this->m_params.cl_max - exess_angle * this->m_params.cl_a_stall;
		cl -= exess_angle * this->m_params.cl_a_stall;
	}
	if( cl < this->m_params.cl_min ) {
		cl = this->m_params.cl_min;
	}*/
	return cl;
}

/*Vector3 Fms7Aero::getUp( const Quaternion &area )
{
  return quatRotate( area, Vector3(0,0,-1) );
}
Vector3 Fms7Aero::getForward( const Quaternion &area )
{
  return quatRotate( area, Vector3(1,0,0) );
}*/

/*
 * Calculate the force that wind is makeing on an orientated surface
 *
 * @param[out] force The caculated force [N]
 * @param[in] wind Directio of wind, length is speed [m/s]
 * @param[in] area Vector perpendicular to surface, length is area [m2]
 */
/*void Fms7Aero::calculateWindForces( ModelSimulator_forces_t &result, const Vector3 &wind, const Quaternion &area, bool addLiftAndDrag )
{
	// todo:remove:
	assert( result.size = 28 );
	static const float rho_half = .5f * 1.2f; // half of air density [kg/m3]

	Vector3 up = getUp(area);
	Vector3 up_unit(up); up_unit.normalize();

	// calculate forced due to dynamic pressure on wing are
	float perp_wind = wind.dot(up_unit); // wind component perp to area [m/s]]
	float q = .5f * rho_half * perp_wind * perp_wind ;   // dynamic pressure [N/m2]
	Vector3 force = q*up;

	// fill in hte results so far
	result.size = 28;
	result.force_x = force.getX();
	result.force_y = force.getY();
	result.force_z = -force.getZ();
	result.torque_x = 0;
	result.torque_y = 0;
	result.torque_z = 0;

	// If not to add Lift and drag, then we're done
	if( addLiftAndDrag == false ) {
		return;
	}

	Vector3 forward_unit = getForward( area ); forward_unit.normalize();

}
*/

/*
 * Calculate lift and drag of main airfoil
 *
 */
/*void calculateLiftAndDrag( Vector3 &force, const Vector3 &wind )
{
	// wind speed perpendicular to surface
	float perp_wind = wind.dot(area.normalized()); // wind component perp to area [m/s]]
	float q = rho_half * perp_wind * perp_wind ;   // dynamic pressure [N/m2]
	force = q*A;
}*/

/*float Fms7Aero::calculateLift(const ModelSimulator_environment_t &environment)
{
	const float rho = 1.2f;

	float alpha = -atan2( -environment.linearVelocityZ, -environment.linearVelocityX ); // get apparent angle of attack (of airplane)
	float cl = getLiftCoefficient( alpha + m_params.alpha0 );

	//return alpha;
	return 2 * cl * ( rho * environment.linearVelocityX * environment.linearVelocityX * m_params.b * m_params.c );
}*/

/*float Fms7Aero::calculateThrust(const ModelSimulator_environment_t &environment)
{
	//
	// calculate the forward (pos x-axis) thrust from motor
	// channel x [-0.8,0.8] -> [ 0, max_thrust ]
	// note: 1/(1.6) = .625
	//
	float thrust = this->m_params.thrust_max * ( .625f* environment.channel[CHANNEL_THRUST] + .5f );
	if( thrust > this->m_params.thrust_max ) thrust = this->m_params.thrust_max;
	if( thrust < 0 ) thrust = 0;

	return thrust;
}*/

/*
 * An (effecive) oriented area at origo is represented with a vector who is perpendicular to the area
 * and a length that represents the area, and a wind vector
void Fms7Aero::calculateWingForces( ModelSimulator_forces_t &forces, const ModelSimulator_environment_t &environment, float area_x, float area_y, float area_z  )
{
	forces
}*/